﻿#include "AlgoCDP.h"
#include <cmath>
#include <numeric>
#include <algorithm>
#include <iostream>
#include <random>

#define M_PI 3.14159265358979323846

static std::vector<double> missedDetectionProbability(
	const double& target_probability,
	const std::vector<double>& sonar_distribution)
{
	std::vector<double> cdp_results;
	cdp_results.reserve(sonar_distribution.size());

	for (const auto& sonar : sonar_distribution) {
		double cdp = target_probability * sonar;
		double fan_cdp = 1.0 - cdp;
		cdp_results.push_back(fan_cdp);
	}

	return cdp_results;
}

static double cumulativeProduct(
	const std::vector<double>& vec,
	size_t start_idx,
	size_t end_idx)
{
	double product = 1.0;
	for (size_t i = start_idx; i <= end_idx; ++i) {
		product *= vec[i];
	}
	return product;
}

static std::vector<double> missedcpcd(const std::vector<double>& sonar_distribution)
{
	std::vector<double> miss_results;
	miss_results.reserve(sonar_distribution.size());

	for (const auto& sonar : sonar_distribution) {
		double fan_cdp = 1.0 - sonar;
		miss_results.push_back(fan_cdp);
	}
	return miss_results;
}

class algo::CDPPrivate
{
public:
	CDP* q;
};

algo::CDP::CDP() : d(new algo::CDPPrivate)
{
	d->q = this;
}

algo::CDP::~CDP()
{
	delete d;
}

std::vector<double> algo::CDP::calculateCDP(
	const double& target_probability,
	const std::vector<double>& sonar_distribution,
	double frequency,
	double time) const
{
	std::vector<double> s_cdps_values = missedDetectionProbability(target_probability, sonar_distribution);
	double a = 1.0 - std::exp(-frequency * time);
	std::vector<double> cdp_results;
	cdp_results.reserve(s_cdps_values.size());

	for (size_t i = 0; i < s_cdps_values.size(); ++i) {
		double current_a_cdp;
		if (i == 0) {
			current_a_cdp = 1.0 - s_cdps_values[0];
		}
		else {
			double product_term = cumulativeProduct(s_cdps_values, 0, i);
			double min_value_term = *std::min_element(s_cdps_values.begin(), s_cdps_values.begin() + i + 1);
			current_a_cdp = 1.0 - std::pow(a, static_cast<double>(i)) * product_term - std::pow(1.0 - a, static_cast<double>(i)) * min_value_term;
		}
		cdp_results.push_back(current_a_cdp);
	}
	return cdp_results;
}

std::vector<double> algo::CDP::calculateCPCD(
	const std::vector<double>& sonar_distribution,
	double frequency,
	double time) const
{
	std::vector<double> s_cdps_values = missedcpcd(sonar_distribution);
	double a = 1.0 - std::exp(-frequency * time);
	std::vector<double> cpcd_results;
	cpcd_results.reserve(s_cdps_values.size());

	for (size_t i = 0; i < s_cdps_values.size(); ++i) {
		double current_a_cdp;
		if (i == 0) {
			current_a_cdp = 1.0 - s_cdps_values[0];
		}
		else {
			double product_term = cumulativeProduct(s_cdps_values, 0, i);
			double min_value_term = *std::min_element(s_cdps_values.begin(), s_cdps_values.begin() + i + 1);
			current_a_cdp = 1.0 - std::pow(a, static_cast<double>(i)) * product_term - std::pow(1.0 - a, static_cast<double>(i)) * min_value_term;
		}
		cpcd_results.push_back(current_a_cdp);
	}
	return cpcd_results;
}